/*
 Pins.c
 Controls a Mobot research vehicle movement and sonar
 via Raspberry Pi
 */

#include <stdio.h>
#include <stdlib.h>
#include "global.h"
#include <math.h>
#include <wiringPi.h>
#include <unistd.h>
#include <softPwm.h>
#include <pthread.h>

//Signal flag
int volatile GO_FLAG = 1;

#define LEFT_MOTOR_PWM 7
#define RIGHT_MOTOR_PWM 0
#define LEFT -1
#define RIGHT 1
int direction = 1;
void wander();
double calcTime();
void setup();
long microsecondsToInches(long microseconds);

//sonar & wander functions
void* run_sonar();
void* run_wander();

//Formula provided by Radio Shack
long microsecondsToInches(long microseconds) {
	return microseconds / 74 / 2;
}

//Movement algorithm for vehicle
void* run_wander() {
	//pins 1 and pins 4 are used as outputs for LED indications
	//for testing right and left directions
	pinMode(1, OUTPUT);
	pinMode(4, OUTPUT);
	int amplitude;
	double t, start;
	for (;;) {
		t = calcTime();
		//Severity of the turn is randomized
		amplitude = rand() % 10;
		//Timer (no interrupts)
		start = millis();

		//locks on to the driving thread
		//and will not check for sonar thread and collision detection
		if (GO_FLAG) {
			while (millis() - start < 500) {
				if (direction > 0) {
					//turn on pin 1 for LED indication
					digitalWrite(4, 0);
					digitalWrite(1, 1);

					// Left turn
					softPwmWrite(RIGHT_MOTOR_PWM, 13 + amplitude);
					softPwmWrite(LEFT_MOTOR_PWM, 0);

				} else {
					// Right turn
					// 50% of the time, increase amplitude by 1
					if (amplitude % 2 == 0) {
						amplitude++;
					}
					//turn on pin 1 for LED indication
					digitalWrite(1, 0);
					digitalWrite(4, 1);

					softPwmWrite(RIGHT_MOTOR_PWM, 0);
					softPwmWrite(LEFT_MOTOR_PWM, 32 + amplitude);
				}
			}
			//Change direction
			direction *= -1;

		}
	}
}

//Gather information from sonar
void* run_sonar() {
	float right_duration, left_duration, left_inches, right_inches;
	for (;;) {

		//Right sensor
		pinMode(2, OUTPUT);
		digitalWrite(2, LOW);
		delayMicroseconds(2);
		digitalWrite(2, HIGH);
		delayMicroseconds(5);
		digitalWrite(2, LOW);
		pinMode(2, INPUT);
		right_duration = pulseIn(2, HIGH, 5000); //distance of pulse from object detected to the sonar sensor

		//left sensor
		pinMode(3, OUTPUT);
		digitalWrite(3, LOW);
		delayMicroseconds(2);
		digitalWrite(3, HIGH);
		delayMicroseconds(5);
		digitalWrite(3, LOW);
		pinMode(3, INPUT);
		left_duration = pulseIn(3, HIGH, 5000); //distance of pulse from object detected to the sonar sensor

		// convert the time into a distance right sensor
		right_inches = microsecondsToInches(right_duration);

		// convert the time into a distance left sensor
		left_inches = microsecondsToInches(left_duration);
		delay(100);

		printf("The object distance from RIGHT side:%2.2f inches\n", right_inches);

		printf("The object distance from LEFT side:%2.2f inches\n", left_inches);

		if ((left_inches <= 25 && left_inches > 6)
				|| (right_inches <= 25 && right_inches > 6)) {
			//Stop if object seen
			softPwmWrite(LEFT_MOTOR_PWM, 0);
			softPwmWrite(RIGHT_MOTOR_PWM, 0);
			GO_FLAG = 0;
		} else {
			GO_FLAG = 1;
		}
	}
}

//Provided by Raspberry Pi enthusiast
int pulseIn(int pin, int level, int timeout) {
	struct timeval tn, t0, t1;
	long micros;
	gettimeofday(&t0, NULL);
	micros = 0;
	while (digitalRead(pin) != level) {
		gettimeofday(&tn, NULL);
		if (tn.tv_sec > t0.tv_sec)
			micros = 1000000L;
		else
			micros = 0;
		micros += (tn.tv_usec - t0.tv_usec);
		if (micros > timeout)
			return 0;
	}
	gettimeofday(&t1, NULL);
	while (digitalRead(pin) == level) {
		gettimeofday(&tn, NULL);
		if (tn.tv_sec > t0.tv_sec)
			micros = 1000000L;
		else
			micros = 0;
		micros = micros + (tn.tv_usec - t0.tv_usec);
		if (micros > timeout)
			return 0;
	}
	if (tn.tv_sec > t1.tv_sec)
		micros = 1000000L;
	else
		micros = 0;
	micros = micros + (tn.tv_usec - t1.tv_usec);
	return micros;
}


int main() {
	//Setup for the wiringPi
	if (wiringPiSetup() == -1) {
		printf("Fail to setup wiringPi \n");
		exit(1);
	}

	pinMode(0, OUTPUT);
	pinMode(7, OUTPUT);

	if (softPwmCreate(RIGHT_MOTOR_PWM, 0, 100) == -1) {
		printf("Fail to create softPwm \n");
		exit(1);
	}

	if (softPwmCreate(LEFT_MOTOR_PWM, 0, 100) == -1) {
		printf("Fail to create softPwm \n");
		exit(1);
	}

	//Threads references for sonar and wander
	pthread_t sonar_thread;
	pthread_t wander_thread;

	//Creation of the sonar & wander threads
	pthread_create(&sonar_thread, NULL, &run_sonar, NULL);
	pthread_create(&wander_thread, NULL, &run_wander, NULL);

	//Joining of sonar & wander threads
	pthread_join(sonar_thread, NULL);
	pthread_join(wander_thread, NULL);

	return 0;
}

